#include "kalman_filter.h"

float KalmanFilter(KFS *kfs_ptr, float inData)
{
    kfs_ptr->p = kfs_ptr->p + kfs_ptr->q;
    kfs_ptr->kGain = kfs_ptr->p / (kfs_ptr->p + kfs_ptr->r);                      // 计算卡尔曼增益
    inData = kfs_ptr->prevData + (kfs_ptr->kGain * (inData - kfs_ptr->prevData)); // 计算本次滤波估计值
    kfs_ptr->p = (1 - kfs_ptr->kGain) * kfs_ptr->p;                               // 更新测量方差
    kfs_ptr->prevData = inData;
    return inData; // 返回估计值
}
